#include "dds_test_tool/dds_test_publisher.h"
#include <chrono>

#include "global_defines.h"
#include "test_utils.h"

DDSTestPublisher::DDSTestPublisher(bool isRealiable, int historyDepth, double blockSize, double hz) :
    Node("dds_test_pub")
{
    binArray = TestUtils::generate_random_array(blockSize * 1024 * 1024);

    rclcpp::QoS profile(historyDepth);
    if(isRealiable){
        profile.reliability(RMW_QOS_POLICY_RELIABILITY_RELIABLE);
    }else{
        profile.reliability(RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT);
    }

    dataPub = create_publisher<dds_test_interface::msg::TestData>(DDS_TEST_TOPIC , profile);

    pubTimer = create_wall_timer(
        std::chrono::milliseconds((int64_t)(1000 / hz)),
        std::bind(&DDSTestPublisher::onPublishTimerCallback, this)
    );


}


DDSTestPublisher::~DDSTestPublisher()
{

}

void DDSTestPublisher::onPublishTimerCallback()
{
    RCLCPP_INFO(get_logger(), "Publish Msg ... ");

    auto msg = dds_test_interface::msg::TestData();
    msg.header.stamp = get_clock()->now();
    msg.header.frame_id = "dds_test";

    msg.msg_cnt = msgCnt++;
    msg.payload = binArray;
    msg.length = binArray.size();

    dataPub->publish(msg);
}
